// ===============================================================================================
// =                                UAVX Quadrocopter Controller                                 =
// =                           Copyright (c) 2008 by Prof. Greg Egan                             =
// =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
// ===============================================================================================

//    This is part of UAVX.

//    UAVX is free software: you can redistribute it and/or modify it under the terms of the GNU
//    General Public License as published by the Free Software Foundation, either version 3 of the
//    License, or (at your option) any later version.

//    UAVX is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
//    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//    See the GNU General Public License for more details.

//    You should have received a copy of the GNU General Public License along with this program.
//    If not, see http://www.gnu.org/licenses/

#include "UAVX.h"


// Adapted from iNav

// Declination lookup from iNav - Out of date?

#define VAR_TABLE_RES		10.00000f
#define VAR_TABLE_LON_MIN	-180.00000f
#define VAR_TABLE_LON_MAX	180.00000f
#define VAR_TABLE_LAT_MIN	-90.00000f
#define VAR_TABLE_LAT_MAX	90.00000f

static const real32 var_table[19][37] = { { 149.21545f, 139.21545f, 129.21545f,
		119.21545f, 109.21545f, 99.21545f, 89.21545f, 79.21545f, 69.21545f,
		59.21545f, 49.21545f, 39.21545f, 29.21545f, 19.21545f, 9.21545f,
		-0.78455f, -10.78455f, -20.78455f, -30.78455f, -40.78455f, -50.78455f,
		-60.78455f, -70.78455f, -80.78455f, -90.78455f, -100.78455f,
		-110.78455f, -120.78455f, -130.78455f, -140.78455f, -150.78455f,
		-160.78455f, -170.78455f, 179.21545f, 169.21545f, 159.21545f,
		149.21545f }, { 129.49173f, 117.24379f, 106.10409f, 95.92286f,
		86.51411f, 77.69583f, 69.30955f, 61.22803f, 53.35622f, 45.62814f,
		38.00071f, 30.44524f, 22.93795f, 15.45138f, 7.94874f, 0.38309f,
		-7.29841f, -15.14665f, -23.20230f, -31.49030f, -40.01947f, -48.78754f,
		-57.79007f, -67.03110f, -76.53295f, -86.34401f, -96.54357f,
		-107.24276f, -118.57946f, -130.70159f, -143.72988f, -157.69312f,
		-172.44642f, 172.37735f, 157.31228f, 142.89420f, 129.49173f }, {
		85.58549f, 77.67225f, 71.30884f, 65.86377f, 60.92589f, 56.17990f,
		51.37001f, 46.30480f, 40.87538f, 35.06788f, 28.95999f, 22.69753f,
		16.44959f, 10.34723f, 4.42361f, -1.41547f, -7.36729f, -13.65814f,
		-20.45428f, -27.80115f, -35.61613f, -43.73164f, -51.96263f, -60.16701f,
		-68.28139f, -76.33426f, -84.45291f, -92.88710f, -102.08596f,
		-112.90534f, -127.14324f, -148.72029f, 177.01697f, 138.42469f,
		112.13402f, 96.22709f, 85.58549f }, { 47.63660f, 46.34868f, 44.88668f,
		43.46724f, 42.13334f, 40.75578f, 39.03991f, 36.60567f, 33.13133f,
		28.48297f, 22.78165f, 16.40811f, 9.93011f, 3.92458f, -1.26916f,
		-5.73344f, -9.95561f, -14.59663f, -20.17634f, -26.83800f, -34.30473f,
		-42.04900f, -49.54378f, -56.41737f, -62.46557f, -67.58445f, -71.67147f,
		-74.48888f, -75.41488f, -72.72854f, -60.65739f, -20.78150f, 26.31817f,
		42.64478f, 47.39885f, 48.29568f, 47.63660f }, { 30.97061f, 31.18244f,
		30.92007f, 30.51421f, 30.19783f, 30.07382f, 29.96272f, 29.32724f,
		27.44230f, 23.71829f, 17.99066f, 10.70786f, 2.93356f, -4.02123f,
		-9.25421f, -12.72420f, -15.16719f, -17.68585f, -21.36741f, -26.80526f,
		-33.62817f, -40.77079f, -47.22749f, -52.37678f, -55.85085f, -57.33789f,
		-56.38997f, -52.20324f, -43.65581f, -30.23589f, -13.76879f, 1.84396f,
		13.87138f, 22.01689f, 27.05355f, 29.80197f, 30.97061f }, { 22.35701f,
		22.87747f, 22.95380f, 22.77246f, 22.50130f, 22.37009f, 22.48347f,
		22.51381f, 21.59756f, 18.63829f, 12.92222f, 4.75452f, -4.29892f,
		-12.13023f, -17.44827f, -20.36076f, -21.71093f, -22.23838f, -22.95576f,
		-25.55066f, -30.56100f, -36.48539f, -41.58017f, -44.82620f, -45.65465f,
		-43.72977f, -38.84914f, -30.98997f, -21.09926f, -11.31468f, -3.00404f,
		3.97652f, 9.93802f, 14.84528f, 18.57977f, 21.04644f, 22.35701f }, {
		16.83374f, 17.31701f, 17.52984f, 17.52512f, 17.27497f, 16.89826f,
		16.64631f, 16.52197f, 15.82359f, 13.20018f, 7.50544f, -1.01430f,
		-10.31063f, -17.86218f, -22.52054f, -24.77956f, -25.51811f, -24.67883f,
		-22.16922f, -20.17352f, -21.46713f, -25.48482f, -29.63408f, -31.91043f,
		-31.42054f, -28.23519f, -22.87573f, -15.95318f, -8.88587f, -3.41726f,
		0.43088f, 3.87433f, 7.44305f, 10.85242f, 13.73685f, 15.75505f,
		16.83374f }, { 13.16653f, 13.42584f, 13.56856f, 13.64914f, 13.49908f,
		13.04415f, 12.54312f, 12.17294f, 11.33249f, 8.62333f, 2.85124f,
		-5.49975f, -14.06214f, -20.49962f, -23.98868f, -24.97282f, -24.13471f,
		-21.32697f, -16.43179f, -11.33581f, -9.07326f, -10.72561f, -14.43596f,
		-17.29905f, -17.69300f, -15.77796f, -12.24267f, -7.56947f, -3.00400f,
		-0.11834f, 1.39928f, 3.14408f, 5.63638f, 8.32414f, 10.72028f,
		12.39111f, 13.16653f }, { 10.90437f, 10.88236f, 10.81163f, 10.86511f,
		10.80010f, 10.39894f, 9.91702f, 9.49494f, 8.41184f, 5.36760f,
		-0.47690f, -8.26124f, -15.66061f, -20.76090f, -22.78986f, -21.89942f,
		-18.89020f, -14.52921f, -9.53528f, -4.91597f, -1.91353f, -1.76484f,
		-4.25201f, -7.17489f, -8.55660f, -8.15241f, -6.39303f, -3.54345f,
		-0.64444f, 0.81130f, 1.15035f, 2.11074f, 4.17898f, 6.59237f, 8.80142f,
		10.33532f, 10.90437f }, { 9.69156f, 9.50282f, 9.23338f, 9.25605f,
		9.28344f, 8.98186f, 8.56848f, 8.04846f, 6.56771f, 3.06569f, -2.75457f,
		-9.75616f, -15.91853f, -19.62874f, -20.10653f, -17.62100f, -13.38807f,
		-8.80249f, -4.83206f, -1.62618f, 0.85458f, 1.72352f, 0.35339f,
		-2.00271f, -3.58526f, -3.93648f, -3.30170f, -1.78891f, -0.13334f,
		0.41999f, 0.14853f, 0.72192f, 2.61852f, 5.02104f, 7.34668f, 9.06648f,
		9.69156f },
		{ 8.99220f, 9.02256f, 8.80901f, 8.93941f, 9.15462f, 8.99412f, 8.49413f,
				7.54605f, 5.38233f, 1.29060f, -4.51973f, -10.72800f,
				-15.61393f, -17.87879f, -17.08037f, -13.87843f, -9.54744f,
				-5.34026f, -2.15287f, 0.11672f, 2.02473f, 3.04733f, 2.31093f,
				0.47960f, -0.96646f, -1.54925f, -1.53032f, -1.00772f,
				-0.42112f, -0.63497f, -1.37540f, -1.17754f, 0.48492f, 2.94585f,
				5.61932f, 7.87900f, 8.99220f }, { 8.04368f, 8.88487f, 9.24731f,
				9.76871f, 10.30385f, 10.32830f, 9.60849f, 7.94603f, 4.81731f,
				-0.08975f, -6.10101f, -11.65411f, -15.25677f, -16.15822f,
				-14.52060f, -11.22653f, -7.24580f, -3.44558f, -0.61333f,
				1.25309f, 2.76805f, 3.73177f, 3.36695f, 1.97948f, 0.74967f,
				0.13075f, -0.18979f, -0.42444f, -0.85420f, -1.93961f,
				-3.28866f, -3.62225f, -2.35247f, 0.08639f, 3.11990f, 6.05468f,
				8.04368f }, { 6.44887f, 8.52898f, 10.00772f, 11.25814f,
				12.19628f, 12.38325f, 11.43950f, 9.05774f, 4.87180f, -1.07081f,
				-7.58681f, -12.76972f, -15.34699f, -15.21292f, -13.06213f,
				-9.80318f, -6.10115f, -2.52848f, 0.26062f, 2.11110f, 3.47313f,
				4.40612f, 4.43031f, 3.60979f, 2.68625f, 2.01876f, 1.34276f,
				0.35243f, -1.18272f, -3.35038f, -5.50704f, -6.45531f,
				-5.60558f, -3.25332f, 0.01287f, 3.49652f, 6.44887f }, {
				4.61309f, 7.91484f, 10.67459f, 12.86217f, 14.28349f, 14.59916f,
				13.44634f, 10.43327f, 5.19620f, -1.95110f, -9.25904f,
				-14.43995f, -16.45824f, -15.72227f, -13.22423f, -9.85034f,
				-6.14231f, -2.53072f, 0.48676f, 2.68447f, 4.29956f, 5.52570f,
				6.19623f, 6.20084f, 5.73301f, 4.92006f, 3.60085f, 1.53163f,
				-1.39438f, -4.87688f, -7.94536f, -9.43097f, -8.85100f,
				-6.51552f, -3.06017f, 0.84306f, 4.61309f }, { 3.21875f,
				7.41586f, 11.19354f, 14.28261f, 16.32057f, 16.89633f,
				15.56524f, 11.80300f, 5.20653f, -3.55470f, -11.97809f,
				-17.43029f, -19.22616f, -18.17302f, -15.37952f, -11.69571f,
				-7.65025f, -3.63286f, -0.01295f, 2.99661f, 5.46716f, 7.56207f,
				9.25413f, 10.31810f, 10.49165f, 9.54146f, 7.26006f, 3.54973f,
				-1.33752f, -6.50635f, -10.55663f, -12.38793f, -11.79132f,
				-9.25449f, -5.49875f, -1.18292f, 3.21875f }, { 2.52051f,
				7.33138f, 11.79037f, 15.58977f, 18.29190f, 19.30865f,
				17.85567f, 12.95067f, 3.98293f, -7.53431f, -17.54806f,
				-23.19596f, -24.60407f, -23.07402f, -19.77569f, -15.49637f,
				-10.73649f, -5.85097f, -1.12175f, 3.26826f, 7.26895f,
				10.88888f, 14.03678f, 16.41453f, 17.52792f, 16.76756f,
				13.54324f, 7.61980f, -0.22282f, -7.89770f, -13.19396f,
				-15.21847f, -14.33135f, -11.37454f, -7.18383f, -2.40863f,
				2.52051f }, { 2.02983f, 7.36989f, 12.37081f, 16.71056f,
				19.89761f, 21.11515f, 18.97400f, 11.40232f, -2.70274f,
				-18.81527f, -29.66002f, -33.88571f, -33.57644f, -30.58814f,
				-26.04521f, -20.59815f, -14.64262f, -8.44459f, -2.20066f,
				3.93897f, 9.85468f, 15.42277f, 20.45728f, 24.64081f, 27.44761f,
				28.04182f, 25.18387f, 17.54097f, 5.49796f, -6.72811f,
				-14.58218f, -17.34691f, -16.39748f, -13.14509f, -8.58967f,
				-3.38880f, 2.02983f }, { 0.42359f, 5.67280f, 10.45897f,
				14.18719f, 15.84273f, 13.44387f, 3.31185f, -17.06908f,
				-37.86930f, -48.41039f, -50.74957f, -48.66334f, -44.20871f,
				-38.40471f, -31.78712f, -24.66267f, -17.22360f, -9.60278f,
				-1.90231f, 5.78964f, 13.38658f, 20.79088f, 27.87654f,
				34.46122f, 40.25514f, 44.75763f, 47.03469f, 45.25758f,
				36.18138f, 17.77476f, -1.60905f, -12.11869f, -14.89059f,
				-13.37813f, -9.65915f, -4.84192f, 0.42359f }, { -178.76222f,
				-168.76222f, -158.76222f, -148.76222f, -138.76222f,
				-128.76222f, -118.76222f, -108.76222f, -98.76222f, -88.76222f,
				-78.76222f, -68.76222f, -58.76222f, -48.76222f, -38.76222f,
				-28.76222f, -18.76222f, -8.76222f, 1.23778f, 11.23778f,
				21.23778f, 31.23778f, 41.23778f, 51.23778f, 61.23778f,
				71.23778f, 81.23778f, 91.23778f, 101.23778f, 111.23778f,
				121.23778f, 131.23778f, 141.23778f, 151.23778f, 161.23778f,
				171.23778f, -178.76222f }

};

real32 getMagVariation(void) {
	// Odd mix of variable types - needs checking/rethink - gke
	// If the values exceed valid ranges, return zero as default
	// as we have no way of knowing what the closest real value
	// would be.

	real32 lat, lon;
	int16 min_lat, min_lon, min_lat_index, min_lon_index;
	real32 var_sw, var_se, var_ne, var_nw;
	real32 var_max, var_min;

	//Melbourne MEL 11.61 Lat -37.673332 Lon 144.843333	returns 10.7
	//San Francisco SFO 13.34 Lat 37.618806 Lon -122.375416 returns 13.6

	lat = (real64) GPS.lat * 1e-7;
	lon = (real64) GPS.lon * 1e-7;

	if (lat < -90.0f || lat > 90.0f || lon < -180.0f || lon > 180.0f)
		return 0.0f;

	// round down to nearest sampling resolution
	min_lat = (int16) (lat / VAR_TABLE_RES) * VAR_TABLE_RES;
	min_lon = (int16) (lon / VAR_TABLE_RES) * VAR_TABLE_RES;

	// for the rare case of hitting the bounds exactly
	// the rounding logic wouldn't fit, so enforce it.

	// limit to table bounds - required for maxima even
	// when table spans full globe range
	if (lat <= VAR_TABLE_LAT_MIN)
		min_lat = VAR_TABLE_LAT_MIN;
	else if (lat >= VAR_TABLE_LAT_MAX)
		min_lat = (int16) (lat / VAR_TABLE_RES) * VAR_TABLE_RES - VAR_TABLE_RES;

	if (lon <= VAR_TABLE_LON_MIN)
		min_lon = VAR_TABLE_LON_MIN;
	else if (lon >= VAR_TABLE_LON_MAX)
		min_lon = (int16) (lon / VAR_TABLE_RES) * VAR_TABLE_RES - VAR_TABLE_RES;

	// find index of nearest low sampling point
	min_lat_index = (-(VAR_TABLE_LAT_MIN) + min_lat) / VAR_TABLE_RES;
	min_lon_index = (-(VAR_TABLE_LON_MIN) + min_lon) / VAR_TABLE_RES;

	var_sw = var_table[min_lat_index][min_lon_index];
	var_se = var_table[min_lat_index][min_lon_index + 1];
	var_ne = var_table[min_lat_index + 1][min_lon_index + 1];
	var_nw = var_table[min_lat_index + 1][min_lon_index];

	// perform bilinear interpolation on the four grid corners
	var_min = ((lon - min_lon) / VAR_TABLE_RES) * (var_se - var_sw) + var_sw;
	var_max = ((lon - min_lon) / VAR_TABLE_RES) * (var_ne - var_nw) + var_nw;

	return ((lat - min_lat) / VAR_TABLE_RES) * (var_max - var_min) + var_min;

} // getMagVariation


